#include "buaa/formation.h"
// #include "vector.h"
#define AGENT_ID 0

using namespace std;
using namespace Eigen;

vector<float> global_goal = vector<float>(3,0);

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "agent");
    ros::NodeHandle nh("~");

    const int LOOPRATE = 30;
    ros::Rate loop_rate(LOOPRATE);  
    ROS_INFO("agent %d node start-----",AGENT_ID);

    double map_resolution;
    int map_N;
    nh.getParam("/map_resolution",map_resolution);
    nh.getParam("/map_N",map_N);

    Formation_Agent formation_agent(AGENT_ID,map_resolution,map_N);

    nh.getParam("/EXEC_TIME",formation_agent.EXEC_TIME);
    nh.getParam("/MAP_VISUAL_TIME",formation_agent.MAP_VISUAL_TIME);

    formation_agent.init(nh);
    
    nh.getParam("/SAFETY_DISTANCE",formation_agent.SAFETY_DISTANCE);
    nh.getParam("/v",formation_agent.v);
    nh.getParam("/local_length",formation_agent.local_length);
    nh.getParam("/LAND_HEIGHT",formation_agent.LAND_HEIGHT);

    nh.getParam("/global_goal",global_goal);
    formation_agent.global_goal[0] = global_goal[0];
    formation_agent.global_goal[1] = global_goal[1];
    formation_agent.global_goal[2] = global_goal[2];

	nh.getParam("/MIN_GOAL_DIS",formation_agent.MIN_GOAL_DIS);
	nh.getParam("/REPLAN_DIST",formation_agent.REPLAN_DIST);
    nh.getParam("/sendpva_count_start",formation_agent.sendpva_count_start);
    nh.getParam("/SET_POINT_CONTROL",formation_agent.SET_POINT_CONTROL);

    nh.getParam("/D435_T265_OFFSET_X",formation_agent.D435_T265_OFFSET_X);
    nh.getParam("/D435_T265_OFFSET_Z",formation_agent.D435_T265_OFFSET_Z);

    nh.getParam("/D435_BASE_OFFSET_X",formation_agent.D435_BASE_OFFSET_X);
    nh.getParam("/D435_BASE_OFFSET_Z",formation_agent.D435_BASE_OFFSET_Z);
	
    nh.getParam("/T265_OFFSET_X",formation_agent.T265_OFFSET_X);
    nh.getParam("/T265_OFFSET_Y",formation_agent.T265_OFFSET_Y);
    nh.getParam("/T265_OFFST265_OFFSET_ZET_Y",formation_agent.T265_OFFSET_Z);
    nh.getParam("/replan_time",formation_agent.replan_time);
    nh.getParam("/NEED_OFFBOARD_READY",formation_agent.NEED_OFFBOARD_READY);
    nh.getParam("/visual_map_range",formation_agent.visual_map_range);
    nh.getParam("/VISUAL_TRANS_CLOUD",formation_agent.VISUAL_TRANS_CLOUD);
    nh.getParam("/ENTER_EXEC_CALLBACK",formation_agent.ENTER_EXEC_CALLBACK);
    nh.getParam("/SAVE_TRAJ",formation_agent.SAVE_TRAJ);
    nh.getParam("/NEED_GLOBAL_POS",formation_agent.NEED_GLOBAL_POS);
    nh.getParam("/TAKEOFF_HEIGHT",formation_agent.TAKEOFF_HEIGHT);
    nh.getParam("/TIME_TAKEOFF",formation_agent.TIME_TAKEOFF);

    bool MULTI_THEREAD;
    nh.getParam("/MULTI_THEREAD",MULTI_THEREAD);


    std::cout << "formation_agent.SAFETY_DISTANCE : " << formation_agent.SAFETY_DISTANCE << endl;
    std::cout << "formation_agent.local_length : " << formation_agent.local_length << endl;
    std::cout << "formation_agent.global_goal[0] : " << formation_agent.global_goal[0] << endl;
    std::cout << "formation_agent.global_goal[1] : " << formation_agent.global_goal[1] << endl;
    std::cout << "formation_agent.global_goal[2] : " << formation_agent.global_goal[2] << endl;
    std::cout << "formation_agent.sendpva_count_start : " << formation_agent.sendpva_count_start << endl;

    if(MULTI_THEREAD)
    {
        ros::AsyncSpinner spinner(3); // Use 3 threads
        spinner.start();
        ros::waitForShutdown();
    }
    else{
        ros::spin();
    }


	return 0;

}
